/* main.c */
#include "app_inc.h"

#define APP_MPU9250_LPIT_CHANNEL  0U

MPU9250_SensorData_T gAppMpu9250SensorDataStruct;
KalmanFilter_InputPosData_T gAppKalmanPosData;
KalmanFilter_InputPosData_T gAppKalmanPosDataOffset =
{
    .AccelForwardVal = 0,
    .AccelDownVal = 0,
    .GyroForwardVal = 0.0
};
float gAppKalmanAngle;

void App_InitSensor(void);
void App_ProcessSensorData(void);

int main(void)
{
    BSP_InitSystem();
    BSP_InitStdioLPUART(115200U);

    printf("\r\n\r\nLPSPI_MPU9250 Example.\r\n");
    printf("Compiled on %s, %s\r\n\r\n", __DATE__, __TIME__);

    App_InitSensor();
    printf("All Ready.\r\n");

    /* 启用周期采样，开始刷新MPU9250的采样值 */
    LPIT_HAL_StartChannel(LPIT0, APP_MPU9250_LPIT_CHANNEL);

    while (1)
    {
        getchar();

        printf("gAppKalmanAngle: %d.%d\r\n", (int)(gAppKalmanAngle*1000)/1000, (int)(gAppKalmanAngle*1000)%1000);
    }
}

void App_InitSensor(void)
{
    LPIT_ChannelConfig_T lpitChannelConfigStruct;

    /* 初始化LPSPI_EDMA传输 */
    MPU9250_Init();
    printf("MPU9250_Init() Done.\r\n");

    /* 读取MPU9250的ID寄存器值，验证传感器已经上线 */
    if ( MPU9250_Probe() )
    {
        printf("MPU9250_Probe() Done.\r\n");
    }
    else
    {
        printf("MPU9250_Probe() Failed.\r\n");
        while(1); /* 别往下走了，别惹麻烦 */
    }

    /* 配置MPU9250的工作方式，转换完成后产生中断 */
    MPU9250_Setup();

    /* 初始化PIT定时器 */
    /* Enable clock for LPIT. */
    PCC->CLKCFG[PCC_LPIT0_INDEX] = PCC_CLKCFG_CGC_MASK | PCC_CLKCFG_PCS(6U);
    /* Configure LPIT. */
    LPIT_HAL_Enable(LPIT0, true);
    LPIT_HAL_Reset(LPIT0);
    LPIT_HAL_GetChannelConfigForNormalTimer(&lpitChannelConfigStruct);
    LPIT_HAL_ConfigChannel(LPIT0, APP_MPU9250_LPIT_CHANNEL, &lpitChannelConfigStruct);
    LPTI_HAL_SetChannelTimeoutValue(LPIT0, APP_MPU9250_LPIT_CHANNEL, BSP_CLK_SPLLDIV2_HZ/200U); /* 指定PIT的中断周期，200Hz, 5ms */
    LPIT_HAL_EnableChannelInterrupt(LPIT0, APP_MPU9250_LPIT_CHANNEL, true);
    /* 解锁NVIC中LPIT的中断向量 */
    NVIC_EnableIRQ(LPIT0_IRQn);
}

/* 获取传感器数据之后的处理流程 */
void App_ProcessSensorData(void)
{

    /* 释放片选信号线 */

    /* 格式化传感器数据 */
    MPU9250_GetSensorData(&gAppMpu9250SensorDataStruct);

    /* 使用Kalman滤波器处理，得到滤波后的倾角 */
    gAppKalmanPosData.AccelForwardVal = -gAppMpu9250SensorDataStruct.AccY;
    gAppKalmanPosData.AccelDownVal = gAppMpu9250SensorDataStruct.AccZ;
    gAppKalmanPosData.GyroForwardVal = -gAppMpu9250SensorDataStruct.GyrX;
    gAppKalmanAngle = KalmanFilter_Process(&gAppKalmanPosData, &gAppKalmanPosDataOffset);

    /* Todo: 使用倾角，通过PID算法，计算电机的调整值 */
}

/*
* LPIT中断服务程序入口
*/
void LPIT0_IRQHandler(void)
{
    /* 周期采样MPU9250传感器通道 */
    if (LPIT_HAL_GetChannelTimeoutFlag(LPIT0, APP_MPU9250_LPIT_CHANNEL))
    {
        LPTI_HAL_ClearChannelTimeoutFlag(LPIT0, APP_MPU9250_LPIT_CHANNEL);
        /* 触发转换 */
        MPU9250_UpdateSensorData(App_ProcessSensorData); /* 在转换结束后调用App_ProcessSensorData()处理数据 */
    }

}

/* EOF. */

